/*
 * Copyright (c) 2006-2021, Halo Team
 *
 * SPDX-License-Identifier: GPLv3
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-10-16     Eywen       the first version
 */


#include <math.h>
#include <string.h>
#include "common.h"
#include "drivers/drv_smart_step.h"
#include "motion_control.h"

#define FRE_VELOCITY_COEF       11
#define JERK_VELOCITY           12
#define JEEK_FREQ               FRE_VELOCITY_COEF*(JERK_V)

static bool motion_control_run_add_task(motion_control_t *motion_control);
static bool motion_control_run_pop_task(motion_control_t *motion_control);


/* 根据给定路径的参数求出本次路径的函数关系
 * para： 用来保存求解的参数
 * f_start： 起始速度
 * f_end： 结束频率
 * f_ave： 平均频率
 * f_start_acc： 开始启动的加频率
 * f_pulse： 本次路程的脉冲个数
 * time_inv： 计算的时间间隔
 * freq_max： 频率最大值限制
 * */
void freq_time_para_set(freq_time_para_t *para, float f_start, float f_end,
                        float f_ave, uint32_t f_pulse, float time_inv,
                        float freq_max, motion_step_type_t step_type, int z_dir)
{
    ASSERT_NULL(para);

    float time_all;
    float time_all_squ;

    if(f_pulse < 1){
        para->a = para->b = para->c = 0;
        goto ret;
    }

    time_all = f_pulse/f_ave;
    time_all_squ = time_all*time_all;
    /* 根据起始加速度：*/
     // * f' = 2*a*t + b */
    //para->b = f_start_acc;
    /* 根据起始速度值：
     * f = a*t^2 + b*t + c */
    para->c = f_start;
    /* 根据结束速度：
     *  f = a*t^2 + b*t + c */
    //para->a = (f_end - para->c - f_start_acc*(time_all))/time_all/time_all;
    float tmp;
    tmp = 0.5*time_all_squ - 1.0/3.0*time_all;
//    para->b = (f_pulse - 1.0/3.0*(f_end - f_start) - f_start*time_all)/tmp;
//    para->a = (f_end - f_start - para->b*time_all)/time_all_squ;
    float a11, a12, a21, a22, b1, b2;
    a11 = 1.0/3*time_all_squ*time_all;
    a12 = 0.5*time_all_squ;
    b1 = f_pulse - para->c*time_all;

    a21 = time_all_squ;
    a22 = time_all;
    b2 = f_end - para->c;
    tmp = a11*a22 - a12*a21;
    para->a = (b1*a22 - a12*b2)/tmp;
    para->b = (a11*b2 - b1*a21)/tmp;

ret:
    para->time_inv = time_inv;
    para->freq_max = freq_max;
    para->freq_end = f_end;
    para->freq_start = f_start;
    para->time_last = 0;
    para->motion_pulse = f_pulse;
    para->time_actual = 0;
    para->task_schdule_time = time_all;

    para->step_type = step_type;
    para->z_dir = z_dir;
#if (ERROR_MOTION_COMPENSATION == true)
    para->actual_pulse = 0;
#endif
//    debug_info("f_start: %d f_end: %d f_ave: %d f_pulse: %u time_all: %d time_all_squ: %d a: %d b: %d c: %d type: %d",
//            (int)f_start, (int)f_end, (int)f_ave,
//            f_pulse,(int)(time_all*100), (int)(time_all_squ*100),
//            (int)(para->a*100), (int)(para->b*100), (int)(para->c*100), para->step_type);

}

/* 从曲线参数中获取新的小段 */
static bool smart_step_line_task_get(smart_step_task_t *new_task,
                                     freq_time_para_t *para)
{
    ASSERT_NULL(new_task);
    ASSERT_NULL(para);

    float time_idx;
    float time_squ;

    /*上次计划的时间超过了目标时间，则不需要在去执行任务了*/
    if(para->time_last >= para->task_schdule_time){
        return false;
    }

#if (ERROR_MOTION_COMPENSATION == true)
    if(para->motion_pulse <= para->actual_pulse){
        /* 校验发现已经计算完毕所有的脉冲了，因此后续不需要添加脉冲了 */
        new_task->freq = para->freq_max;
        new_task->pulse = 0;
        return false;
    }
#endif

    rt_enter_critical();

    time_idx = para->time_last + para->time_inv;

    if(time_idx > para->task_schdule_time){
        time_idx = para->task_schdule_time;
        para->time_inv = time_idx - para->time_last;
    }

    para->time_last = time_idx;
    time_squ = time_idx * time_idx;

    /* f = a*t^2 + b*t + c */
    new_task->freq = para->a * time_squ + para->b*time_idx + para->c;
    /* 脉冲个数等于频率*单位时间 */
    new_task->pulse = new_task->freq*para->time_inv;

    /* 频率大于最大频率，则限制为最大频率输出，脉冲个数依旧保持原来的个数 */
    if(new_task->freq > para->freq_max){
        new_task->freq = para->freq_max;
    }

    /* 误差补偿，减少算法丢步 */
#if (ERROR_MOTION_COMPENSATION == true)

    do{
        int32_t pulse;

        /* 区分最后一次小任务，非最后一次通过频率积分函数求得应走得步数 */
        if(time_idx < para->task_schdule_time){
            /* 根据积分公式，可求出当前时间的总脉冲个数，可用来校验和补偿每段频率的脉冲数 */
            pulse = (para->a * time_squ * time_idx)/3.0 +
                    0.5*(para->b * time_squ) +
                    1.0*(para->c * time_idx);
        }
        /* 最后一步小任务通过精确计算来防止算法丢步 */
        else{
            pulse = para->motion_pulse;
        }

        /* 算法计算出的总脉冲与实际的脉冲差值 */
        int diff = pulse - (para->actual_pulse + new_task->pulse) ;

        new_task->pulse += diff;

        /* 补偿脉冲数 */
        if(new_task->pulse < 0){
            new_task->pulse = 0;
        }
    }while(0);

    if(fabs(new_task->freq) < 200){
        new_task->freq = 200;
    }

    para->actual_pulse += new_task->pulse;
    para->time_actual += (1.0/new_task->freq*new_task->pulse);
#endif

    rt_exit_critical();

    return true;
}


/* 将一个小直线任务分解成xyz三个方向和e轴方向的运动矢量 */
static bool motion_control_line_task_to_xyze_axis(freq_time_para_t *freq_time_para,
                                           smart_step_task_t *new_task,
        smart_step_task_t *axis_x_task, smart_step_task_t *axis_y_task,
        smart_step_task_t *axis_z_task, smart_step_task_t *axis_e_task)
{
    ASSERT_NULL(freq_time_para);
    ASSERT_NULL(new_task);
    ASSERT_NULL(axis_x_task);
    ASSERT_NULL(axis_y_task);
    ASSERT_NULL(axis_z_task);
    ASSERT_NULL(axis_e_task);

    /* 更新小任务的终点位置 */
    float inc_x, inc_y;
    float freq_x, freq_y;

    rt_enter_critical();

    if(freq_time_para->step_type == step_xy){
        inc_x = new_task->pulse*1.0*freq_time_para->sin_alf_x;
        inc_y = new_task->pulse*1.0*freq_time_para->sin_alf_y;
        inc_x = (inc_x > 0)?inc_x: -inc_x;
        inc_y = (inc_y > 0)?inc_y: -inc_y;

        freq_x = new_task->freq*freq_time_para->sin_alf_x;
        freq_y = new_task->freq*freq_time_para->sin_alf_y;

        axis_x_task->pulse = inc_x;
        axis_x_task->freq = freq_x;

        axis_y_task->pulse = inc_y;
        axis_y_task->freq = freq_y;

        axis_z_task->pulse = 0;
        axis_z_task->freq = 0;

        axis_e_task->pulse = 0;
        axis_e_task->freq = 0;
    }
    else if(freq_time_para->step_type == step_z){
        axis_x_task->pulse = 0;
        axis_x_task->freq = 0;

        axis_y_task->pulse = 0;
        axis_y_task->freq = 0;

        axis_z_task->pulse = new_task->pulse;
        axis_z_task->freq = new_task->freq * freq_time_para->z_dir;

        axis_e_task->pulse = 0;
        axis_e_task->freq = 0;
    }
    rt_exit_critical();
    return true;
}


/* 添加一条直线路径和计算策略到task list */
static bool motion_control_add_line_para(motion_control_t *motion_control,
                                         freq_time_para_t *new_para)
{
    ASSERT_NULL(motion_control);
    ASSERT_NULL(new_para);

    return ringbuff_push(motion_control->handle_list, new_para);
}

static bool motion_control_init(motion_control_t *device)
{
    ASSERT_NULL(device);
    ASSERT_NULL(device->ops);

    device->end_location.pulse_x = 0;
    device->end_location.pulse_y = 0;
    device->end_location.pulse_z = 0;
    device->end_location.pulse_e = 0;

    /* 初始化所有的步进电机 */
    device->ops->step_init(device);

    /* 创建gcode处理线程 */
    device->task_add_thread_id = rt_thread_create("add task",
            (void(*)(void *parameter))motion_control_run_add_task, device,
            MOTION_CONTROL_ADD_TASK_STACK_SIZE,
            MOTION_CONTROL_ADD_TASK_THREAD_PRIORITY,
            MOTION_CONTROL_ADD_TASK_THREAD_TIMESLICE);

    /* 创建gcode处理线程 */
    device->task_pop_thread_id = rt_thread_create("pop task",
            (void(*)(void *parameter))motion_control_run_pop_task, device,
            MOTION_CONTROL_POP_TASK_STACK_SIZE,
            MOTION_CONTROL_POP_TASK_THREAD_PRIORITY,
            MOTION_CONTROL_POP_TASK_THREAD_TIMESLICE);

    if((device->task_add_thread_id == RT_NULL) ||
            (device->task_pop_thread_id == RT_NULL)){
        debug_error("task init fail");
    }

    return true;
}

bool motion_control_pop_task(motion_control_t *motion_control, freq_time_para_t *para)
{
    ASSERT_NULL(motion_control);
    ASSERT_NULL(para);

    return ringbuff_pop(motion_control->handle_list, para);
}

static bool motion_control_task_full(motion_control_t *motion_control)
{
    ASSERT_NULL(motion_control);

    return ringbuff_is_full(motion_control->handle_list);
}

static bool motion_control_task_empty(motion_control_t *motion_control)
{
    ASSERT_NULL(motion_control);

    return ringbuff_is_empty(motion_control->handle_list);
}

static bool motion_control_gcode_task_empty(motion_control_t *motion_control)
{
    ASSERT_NULL(motion_control);

    return ringbuff_is_empty(motion_control->gcode_motion_list);
}


static bool motion_control_add_gcode_task(motion_control_t *motion_control,
                                          motion_t *gcode_task)
{
    ASSERT_NULL(motion_control);
    ASSERT_NULL(gcode_task);

    return ringbuff_push(motion_control->gcode_motion_list, gcode_task);
}

static bool motion_control_pop_gcode_task(motion_control_t *motion_control,
                                          motion_t *gcode_task)
{
    ASSERT_NULL(motion_control);
    ASSERT_NULL(gcode_task);

    return ringbuff_pop(motion_control->gcode_motion_list, gcode_task);
}


/* 已经存在的任务数量 */
static int motion_control_task_count(motion_control_t *motion_control)
{
    return ringbuff_used_count(motion_control->handle_list);
}

static bool motion_control_gcode_task_pop(motion_control_t *motion_control,
                                          motion_t *motion)
{
    ASSERT_NULL(motion_control);
    ASSERT_NULL(motion);

    return ringbuff_pop(motion_control->gcode_motion_list, motion);
}

static bool motion_control_start_all_step(motion_control_t *motion_control)
{
    ASSERT_NULL(motion_control);
    ASSERT_NULL(motion_control->motion_step_x.ops);

    motion_control->motion_step_x.ops->start_task(&(motion_control->motion_step_x));
    motion_control->motion_step_y.ops->start_task(&(motion_control->motion_step_y));
    motion_control->motion_step_y_1.ops->start_task(&(motion_control->motion_step_y_1));
    motion_control->motion_step_z.ops->start_task(&(motion_control->motion_step_z));
    motion_control->extruder_step_e.ops->start_task(&(motion_control->extruder_step_e));
    return true;
}

static inline float caculate_line_k(float x1, float y1, float x2, float y2)
{
    float x;
    x = x2 - x1;
    if(fabs(x) < 1){
        return (float)INT16_MAX;
    }
    return (y2 - y1)/x;
}


static inline float caculate_point_distance(float x, float y)
{
    return (float)sqrt(x*x + y*y);
}

/* 运动线程主函数入口
 * 增加一个点任务到点任务队列
 *
 *      s2/\      /s4
 *       /  \    /
 *   s1 /    \  /
 *          s3\/
 *
 *  */
static bool motion_control_run_add_task(motion_control_t *motion_control)
{
    ASSERT_NULL(motion_control);
    motion_t motion;
    pulse_vector_t vector;
    motion_step_t *motion_step_x;
    motion_step_t *motion_step_y;
    motion_step_t *motion_step_y_1;
    motion_step_t *motion_step_z;
    extruder_step_t *extruder_step_e;
    pulse_vector_t next_vector;
    pulse_vector_t *vector_ptr, *next_vector_ptr;
    pulse_vector_t *new_inc;
    float freq, freq_next;
    //float next_distance;
    static float k = 0, next_k = 0;
    float freq_end;
    float new_old_k_rate;
    float freq_start;

    freq_time_para_t para;

    motion_step_x = &(motion_control->motion_step_x);
    motion_step_y = &(motion_control->motion_step_y);
    motion_step_y_1 = &(motion_control->motion_step_y_1);
    motion_step_z = &(motion_control->motion_step_z);
    extruder_step_e = &(motion_control->extruder_step_e);


    pulse_vector_t add_inc_location;
    new_inc = &(add_inc_location);

    debug_info("motion control run add task started");
    for(;;){
        /* 取出任务 */
        /* gcode运动队列为空，则睡下去,参数队列任务也为空 */
        while(ringbuff_is_empty(motion_control->gcode_motion_list) == true){
            //debug_info("gcode_motion_list empty used: %d",
            //        ringbuff_used_count(motion_control->gcode_motion_list));
            rt_thread_yield();
        }
        /* 队列不为空了，把目标位置取出来 */
        motion_control_gcode_task_pop(motion_control, &motion);

        //debug_info("pop task: x: %d y: %d z: %d", (int)motion.x, (int)motion.y, (int)motion.z);

        vector.pulse_x = motion_step_x->ops-> \
                distance_to_pulse(motion_step_x, motion.x);
        vector.pulse_y = motion_step_y->ops-> \
                distance_to_pulse(motion_step_y, motion.y);
        vector.pulse_z = motion_step_z->ops-> \
                distance_to_pulse(motion_step_z, motion.z);
        //vector.pulse_e = extruder_step_e->ops->distance_to_pulse(extruder_step_e, motion.e);
        vector_ptr = &vector;

        freq = motion.speed;

        k = caculate_line_k(motion_control->end_location.pulse_x, motion_control->end_location.pulse_y,
                        vector.pulse_x, vector.pulse_y);

        /* 后面还有gcode运动，可以用于预判速度 */
        if(motion_control_gcode_task_pop(motion_control, &motion) == true){
            next_vector.pulse_x = motion_step_x->ops-> \
                    distance_to_pulse(motion_step_x, motion.x);
            next_vector.pulse_y = motion_step_y->ops-> \
                    distance_to_pulse(motion_step_y, motion.y);
            next_vector.pulse_z = motion_step_z->ops-> \
                    distance_to_pulse(motion_step_z, motion.z);
            next_vector_ptr = &next_vector;
            freq_next = motion.speed;

            next_k = caculate_line_k(vector.pulse_x, vector.pulse_y, next_vector.pulse_x, next_vector.pulse_y);
        }
        else{
            next_vector_ptr = NULL;
            freq_next = 0;
        }

        freq_start = 0;
        /* 当前要走的距离 */
        while(1){
            new_inc->pulse_x = vector.pulse_x - motion_control->end_location.pulse_x;
            new_inc->pulse_y = vector.pulse_y - motion_control->end_location.pulse_y;
            new_inc->pulse_z = vector.pulse_z - motion_control->end_location.pulse_z;
            new_inc->pulse_e = vector.pulse_e - motion_control->end_location.pulse_e;
            para.motion_pulse = caculate_point_distance(new_inc->pulse_x, new_inc->pulse_y);

//            debug_info("inc: %4d\t%4d\t%4d\t%4d", (int)(new_inc->pulse_x), (int)(new_inc->pulse_y), (int)(new_inc->pulse_z),
//                    (int)(para.motion_pulse));

//            debug_info("cur vector: %d %d %d %d", (int)(vector.pulse_x), (int)(vector.pulse_y), (int)(vector.pulse_z),
//                                (int)(vector.pulse_e));
//            debug_info("end_location vector: %d %d %d %d", (int)(motion_control->end_location.pulse_x),
//                    (int)(motion_control->end_location.pulse_y), (int)(motion_control->end_location.pulse_z),
//                    (int)(motion_control->end_location.pulse_e));

            /* 有下一条gcode并且z轴没有变化，则速度才不降为0 */
            if((next_vector_ptr != NULL) && (fabs(next_vector.pulse_z - vector.pulse_z) < 1)){
                new_old_k_rate = fabs((k - next_k)/(1 + k*next_k));
                new_old_k_rate = 1.0/(new_old_k_rate + 1);
                freq_end = freq_next*new_old_k_rate;
//                debug_info("next vect: %d %d %d %d", (int)next_vector.pulse_x, (int)next_vector.pulse_y,
//                        (int)next_vector.pulse_z,
//                        (int)next_vector.pulse_e);
            }
            else{
                freq_end = 0;
            }

            if(para.motion_pulse < 1){
                freq_end = 0;
            }

            if(fabs(new_inc->pulse_z) > 1){
                //debug_info("z: %d ", (int)new_inc->pulse_z);
                int z_dir;
                if(new_inc->pulse_z > 0){
                    //freq = freq;
                    para.motion_pulse = new_inc->pulse_z;
                    z_dir = 1;
                }
                else{
                    //freq = -freq;
                    para.motion_pulse = -new_inc->pulse_z;
                    z_dir = -1;
                }
                freq_start = freq_end = 0;
                freq_time_para_set(&para, freq_start, freq_end, freq,
                        (uint32_t)para.motion_pulse, 0.1, MOTION_Z_MAX_FREQ, step_z, z_dir);

                //debug_info("a: %6d %6d %6d", (int)para.a, (int)para.b, (int)para.c);

                para.sin_alf_x = 0;
                para.sin_alf_y = 0;
            }
            else{
                /* 计算频率二次函数关系式 */
                freq_time_para_set(&para, freq_start, freq_end, freq,
                        para.motion_pulse, 0.1, MOTION_XY_MAX_FREQ, step_xy, 0);
                para.sin_alf_x = new_inc->pulse_x/para.motion_pulse;
                para.sin_alf_y = new_inc->pulse_y/para.motion_pulse;
            }

            /* 任务队列满了（freq_time_para_t），那睡一会儿等下计算呗 */
            while(motion_control->ops->task_full(motion_control) == true){
                //debug_info("handle_list task buff full");
                rt_thread_yield();
            }

            motion_control->end_location = vector;

            if(para.motion_pulse > 1){
                motion_control_add_line_para(motion_control, &para);
                //debug_info("add: motion pulse: %d k: %d", (int)(para.motion_pulse), (int)(k));
                //debug_info("sin_alf_x_y: %d %d", (int)(para.sin_alf_x*100),(int)(para.sin_alf_y*100));
            }

            if(next_vector_ptr == NULL){
                break;
            }

            if(next_vector_ptr != NULL){
                new_inc->pulse_x = next_vector_ptr->pulse_x - vector_ptr->pulse_x;
                new_inc->pulse_y = next_vector_ptr->pulse_y - vector_ptr->pulse_y;
                new_inc->pulse_z = next_vector_ptr->pulse_z - vector_ptr->pulse_z;
                new_inc->pulse_e = next_vector_ptr->pulse_e - vector_ptr->pulse_e;
                k = next_k;
                freq_start = freq_end;
                freq = freq_next;
                vector = next_vector;

                /* 后面还有gcode运动，可以用于预判速度 */
                if(motion_control_gcode_task_pop(motion_control, &motion) == true){
                    next_vector.pulse_x = motion_step_x->ops-> \
                            distance_to_pulse(motion_step_x, motion.x);
                    next_vector.pulse_y = motion_step_y->ops-> \
                            distance_to_pulse(motion_step_y, motion.y);
                    next_vector.pulse_z = motion_step_z->ops-> \
                            distance_to_pulse(motion_step_z, motion.z);
                    next_vector_ptr = &next_vector;
                    freq_next = motion.speed;

                    next_k = caculate_line_k(vector.pulse_x, vector.pulse_y, next_vector.pulse_x, next_vector.pulse_y);

//                    debug_info("next vect: %d %d %d %d", (int)next_vector.pulse_x, (int)next_vector.pulse_y,
//                            (int)next_vector.pulse_z,
//                            (int)next_vector.pulse_e);
                }
                else{
                    next_vector_ptr = NULL;
                    freq_next = 0;
//                    debug_info("cur vector: %d %d %d %d", (int)(vector.pulse_x), (int)(vector.pulse_y), (int)(vector.pulse_z),
//                                                    (int)(vector.pulse_e));
                }
            }
        }
    }
    return false;
}


/* 运动线程主函数入口，从ringbuff中取出任务给硬件定时器提供执行的任务  */
static bool motion_control_run_pop_task(motion_control_t *motion_control)
{
    ASSERT_NULL(motion_control);

    smart_step_task_t task;
    freq_time_para_t para;

    smart_step_task_t task_x, task_y, task_y_1, task_z, task_e;
    smart_step_task_t last_task_x, last_task_y, last_task_z, last_task_e;

    motion_step_t *motion_step_x;
    motion_step_t *motion_step_y;
    motion_step_t *motion_step_y_1;
    motion_step_t *motion_step_z;
    extruder_step_t *extruder_step_e;

    motion_step_x = &(motion_control->motion_step_x);
    motion_step_y = &(motion_control->motion_step_y);
    motion_step_y_1 = &(motion_control->motion_step_y_1);
    motion_step_z = &(motion_control->motion_step_z);
    extruder_step_e = &(motion_control->extruder_step_e);

    //debug_info("motion control run pop task started");
    for(;;){
        /* 计算队列的任务为空(freq_time_para_t list 为空)，则睡下呗 */
        while(motion_control_task_empty(motion_control) == true){
            //debug_info("para task empty");
            rt_thread_yield();
        }
        /* 取出一个计算队列 */
        motion_control_pop_task(motion_control, &para);
//        if(para.step_type == step_z){
            debug_info("a: %d b: %d c: %d  dir:%d", (int)(para.a), (int)(para.b), (int)(para.c), para.z_dir);
//        }

        /* 将计算队列的小任务完全取出来送给步进电机驱动接口取执行  */
        while(true == smart_step_line_task_get(&task, &para)){
            //debug_info("get task freq: %d\t pulse: %d", (int)(task.freq), (int)(task.pulse));

            last_task_x = task_x;
            last_task_y = task_y;
            last_task_z = task_z;
            last_task_e = task_e;

            motion_control_line_task_to_xyze_axis(&para, &task,
                                &task_x, &task_y, &task_z, &task_e);

//            if(para.step_type == step_z){
                debug_info("taskxy: %6d %6d %6d %6d %6d %6d", (int)(task_x.freq), (int)task_x.pulse,
                        (int)task_y.freq, (int)task_y.pulse, (int)task_z.freq, (int)task_z.pulse);
//
//            }

            if(task_z.pulse >= 1){
                /* 等待xy轴走完 */
                while((ringbuff_is_empty(motion_step_x->step_drv->pulse_list) == false) ||
                        (ringbuff_is_empty(motion_step_y->step_drv->pulse_list) == false)){
                    motion_step_x->step_drv->ops->start(motion_step_x->step_drv);
                    motion_step_y->step_drv->ops->start(motion_step_y->step_drv);
                    motion_step_y_1->step_drv->ops->start(motion_step_y_1->step_drv);
                    //debug_info("wait xy complate");
                    rt_thread_yield();
                }
                /* Z轴开始运动 */
                motion_step_z->step_drv->ops-> \
                                add_task(motion_step_z->step_drv, &task_z);
                motion_step_z->step_drv->ops->start(motion_step_z->step_drv);
                continue;
            }

            /* 电机任务队列是满的，等待其中有剩余空间 */
            while((motion_step_x->ops->task_full(motion_step_x) == true) ||
                    (motion_step_y->ops->task_full(motion_step_y) == true)/* ||
                    (motion_step_z->ops->task_full(motion_step_z) == true) ||
                    (extruder_step_e->ops->task_full(&extruder_step_e) == true)*/){
                motion_step_x->step_drv->ops->start(motion_step_x->step_drv);
                motion_step_y->step_drv->ops->start(motion_step_y->step_drv);
                motion_step_y_1->step_drv->ops->start(motion_step_y_1->step_drv);
                rt_thread_yield();
            }
            /* 将脉冲个数和频率送给步进电机驱动任务队列 */
            motion_step_x->step_drv->ops-> \
                    add_task(motion_step_x->step_drv, &task_x);
            motion_step_y->step_drv->ops-> \
                    add_task(motion_step_y->step_drv, &task_y);
            task_y_1 = task_y;
            task_y_1.freq = -task_y_1.freq;
            motion_step_y_1->step_drv->ops-> \
                    add_task(motion_step_y_1->step_drv, &task_y_1);

//            if((task_x.pulse != 0) || (task_y.pulse != 0)){
//                /* 等待z轴走完 */
//                while(ringbuff_is_empty(motion_step_x->step_drv->pulse_list) == false){
//                    /* Z轴开始运动 */
//                    motion_step_z->step_drv->ops->start(motion_step_z->step_drv);
//                    rt_thread_yield();
//                }
//            }

            /* 同步x轴暂停任务时出现新的动作，必须等y轴跑完上次的路 */
            if((last_task_x.pulse == 0) && (task_x.pulse != 0)){
                while(ringbuff_used_count(motion_step_y->step_drv->pulse_list) > 0){
                    motion_step_y->step_drv->ops->start(motion_step_y->step_drv);
                    motion_step_y_1->step_drv->ops->start(motion_step_y_1->step_drv);
                    rt_thread_yield();
                }
            }
            /* 同步y轴暂停任务时出现新的动作，必须等x轴跑完上次的路 */
            if((last_task_y.pulse == 0) && (task_y.pulse != 0)){
                while(ringbuff_used_count(motion_step_x->step_drv->pulse_list) > 0){
                    motion_step_x->step_drv->ops->start(motion_step_x->step_drv);
                    rt_thread_yield();
                }
            }
        }
    }
    return false;
}

static bool motion_control_task_start(motion_control_t *motion_control)
{
    rt_err_t ret1, ret2;
    /* 线程创建成功，开始运行 */
    if((motion_control->task_add_thread_id != NULL) &&
            (motion_control->task_pop_thread_id != NULL)){
        ret1 = rt_thread_startup(motion_control->task_add_thread_id);
        ret2 = rt_thread_startup(motion_control->task_pop_thread_id);
    }

    /* 线程创建成功，开始运行 */
    if((ret1 == RT_EOK) && (ret2 == RT_EOK)){
        debug_info("start pop & push tiny task ok");
        return true;
    }
    debug_error("thread start fail, %d %d", ret1, ret2);
    return false;
}

static bool motion_control_step_init(motion_control_t *motion_control)
{
    motion_step_t *motion_step_x;
    motion_step_t *motion_step_y;
    motion_step_t *motion_step_y_1;
    motion_step_t *motion_step_z;
    extruder_step_t *extruder_step_e;

    motion_step_x = &(motion_control->motion_step_x);
    motion_step_y = &(motion_control->motion_step_y);
    motion_step_y_1 = &(motion_control->motion_step_y_1);
    motion_step_z = &(motion_control->motion_step_z);
    extruder_step_e = &(motion_control->extruder_step_e);

    motion_step_register(motion_step_x, motion_step_ops_get(step_x));
    motion_step_register(motion_step_y, motion_step_ops_get(step_y));
    motion_step_register(motion_step_y_1, motion_step_ops_get(step_y));
    motion_step_register(motion_step_z, motion_step_ops_get(step_z));
    extruder_step_register(extruder_step_e);

    /* 初始化x轴步进电机 */
    motion_step_x->ops->init(motion_step_x, step_x, smart_step_0 ,
            MOTION_STEP_X_PIN_PWM,  MOTION_STEP_X_PIN_EN,
            MOTION_STEP_X_PIN_DIR, STEP_SCHDULE_FREQ);

    /* 初始化y轴步进电机 */
    motion_step_y->ops->init(motion_step_y, step_y, smart_step_1 ,
            MOTION_STEP_Y_PIN_PWM,  MOTION_STEP_Y_PIN_EN,
            MOTION_STEP_Y_PIN_DIR, STEP_SCHDULE_FREQ);

    motion_step_y->step_drv->ops->forward_level_set(motion_step_y->step_drv, true);
    motion_step_y_1->ops->init(motion_step_y_1, step_y, smart_step_1 ,
            MOTION_STEP_Y_1_PIN_PWM,  MOTION_STEP_Y_1_PIN_EN,
            MOTION_STEP_Y_1_PIN_DIR, STEP_SCHDULE_FREQ);
    motion_step_y_1->step_drv->ops->forward_level_set(motion_step_y_1->step_drv, true);

    motion_step_z->ops->init(motion_step_z, step_z, smart_step_2 ,
                MOTION_STEP_Z_PIN_PWM,  MOTION_STEP_Z_PIN_EN,
                MOTION_STEP_Z_PIN_DIR, STEP_SCHDULE_FREQ);
    return true;
}


static motion_control_ops_t s_motion_control_ops = {
    .init = motion_control_init,
    .task_full = motion_control_task_full,
    .task_empty = motion_control_task_empty,
    .task_count = motion_control_task_count,
    //.pop_task = motion_control_pop_task,
    .add_gcode_task = motion_control_add_gcode_task,
    .start = motion_control_task_start,
    .step_init = motion_control_step_init,
    .start_all_step = motion_control_start_all_step,
    .gcode_task_empty = motion_control_gcode_task_empty,
};

motion_control_t *motion_control_create(void)
{
    motion_control_t *motion_control;

    motion_control = MEM_MALLOC(sizeof(motion_control_t));

    if(NULL == motion_control){
        goto err;
    }

    if(false == motion_control_register(motion_control)){
        goto err;
    }

    debug_info("motion control create success");
    return motion_control;
err:
    if(motion_control != NULL){
        MEM_FREE(motion_control);
    }

    debug_error("motion control create error");
    return NULL;
}

bool motion_control_register(motion_control_t *device)
{
    ASSERT_NULL(device);

    MEM_MEMSET(device, 0, sizeof(motion_control_t));

    device->ops = &(s_motion_control_ops);

    /* 创建ringbuff标识空间 */
    device->handle_list = MEM_MALLOC(sizeof(ringbuff_t));
    if(device->handle_list == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }

    /* 创建ringbuff标识空间 */
    device->gcode_motion_list = MEM_MALLOC(sizeof(ringbuff_t));
    if(device->gcode_motion_list == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }

    int ringbuff_size;
    void *ptr;

    /* 创建直线任务计算buff */
    ringbuff_size = MOTION_CONTROL_TASK_BUFF_LEN*sizeof(freq_time_para_t);
    /* 申请存储ringbuff内容的空间 */
    ptr = MEM_MALLOC(ringbuff_size);
    if(ptr == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }
    /* 初始化ringbuff，绑定空间与ringbuff变量 */
    ringbuff_init(device->handle_list, ptr,
                  ringbuff_size, sizeof(freq_time_para_t));

    /* 创建gcode任务buff */
    ringbuff_size = MOTION_CONTROL_GCODE_TASK_BUF_LEN*sizeof(motion_t);
    /* 申请存储ringbuff内容的空间 */
    ptr = MEM_MALLOC(ringbuff_size);
    if(ptr == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }
    /* 初始化ringbuff，绑定空间与ringbuff变量 */
    ringbuff_init(device->gcode_motion_list, ptr,
                  ringbuff_size, sizeof(motion_t));

    debug_info("motion control register success");
    return true;
err:
    if(NULL != device->handle_list){
        if(NULL != device->handle_list->addr){
            ringbuff_destroy(device->handle_list);
        }
        MEM_FREE(device->handle_list);
        device->handle_list = NULL;
    }

    if(NULL != device->gcode_motion_list){
        if(NULL != device->gcode_motion_list->addr){
            ringbuff_destroy(device->gcode_motion_list);
        }
        MEM_FREE(device->gcode_motion_list);
        device->gcode_motion_list = NULL;
    }

    return false;
}




